﻿#include "plinesfit_qt.h"

inline static void mean_xy(const QVector<QPointF> &line, double &mean_x, double &mean_y)
{
    int size = line.size();
    mean_x = 0.0;
    mean_y = 0.0;
    for(int i = 0; i < size; i++)
    {
        mean_x += line[i].x();
        mean_y += line[i].y();
    }
    mean_x /= size;
    mean_y /= size; //至此，计算出了 x y 的均值
}

inline static void weightedMean_xy(const QVector<QPointF> &line,
                            double &mean_x, double &mean_y,
                            double tau, double a, double b, double c)
{
    int size = line.size();
    mean_x = 0.0;
    mean_y = 0.0;
    double sum_wx = 0, sum_wy = 0, sum_w = 0;
    for(int i = 0; i < size; i++)
    {
        double x = line[i].x(), y = line[i].y();
        double d = abs(a * x + b * y + c);
        double w = d < tau ? 1 : tau / d;
        sum_wx += w * w * x;
        sum_wy += w * w * y;
        sum_w += w * w;
    }
    mean_x = sum_wx / sum_w;
    mean_y = sum_wy / sum_w; //至此，计算出了 x y 的均值
}

inline static void Dxxyyxy(const QVector<QPointF> &line,
                    double mx, double my,
                    double &Dxx, double &Dxy, double &Dyy)
{
    int size = line.size();
    for(int i = 0; i < size; i++)
    {
        double x = line[i].x();
        double y = line[i].y();
        Dxx += (x - mx) * (x - mx);
        Dxy += (x - mx) * (y - my);
        Dyy += (y - my) * (y - my);
    }
}

inline static void weightedDxxxyyy(const QVector<QPointF> &line,
                            double mx, double my,
                            double &Dxx, double &Dxy, double &Dyy,
                            double tau, double a, double b, double c)
{
    int size = line.size();
    for(int i = 0; i < size; i++)
    {
        double x = line[i].x(), y = line[i].y();
        double d = abs(a * x + b * y + c);
        double w = d < tau ? 1 : tau / d;
        w = w * w;
        Dxx += w * (x - mx) * (x - mx);
        Dxy += w * (x - mx) * (y - my);
        Dyy += w * (y - my) * (y - my);
    }
}


/**
* @brief plinesFit 平行线拟合算法 最小二乘法拟合 ax + by + c = 0 型直线
* @param [in] line1 第一条直线的数据点
* @param [in] line2 第二条直线的数据点
* @param [out] a 输出拟合系数
* @param [out] b 输出拟合系数
* @param [out] c1 输出拟合系数，第一条直线的方程为 ax + by + c1 = 0
* @param [out] c2 输出拟合系数，第二条直线的方程为 ax + by + c2 = 0
* @return true 表示拟合成功，false 表示拟合失败
*/
bool plinesFit(const QVector<QPointF> &line1, const QVector<QPointF> &line2,
               double &a, double &b, double &c1, double &c2)
{

    if(line1.size() < 2 ||  line2.size() <= 0)
    {
        a = 0;
        b = 0;
        c1 = 0;
        c2 = 0;
        return false;
    }
    double x_mean_1 = 0, x_mean_2 = 0;
    double y_mean_1 = 0, y_mean_2 = 0;
    mean_xy(line1, x_mean_1, y_mean_1);
    mean_xy(line2, x_mean_2, y_mean_2);

    double Dxx = 0, Dxy = 0, Dyy = 0;
    Dxxyyxy(line1, x_mean_1, y_mean_1, Dxx, Dxy, Dyy);
    Dxxyyxy(line2, x_mean_2, y_mean_2, Dxx, Dxy, Dyy);

    double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
    double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );

    if(fabs(den) < 1e-5)
    {
        if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向，无法拟合
        {
            return false;
        }
        else
        {
            a = 1;
            b = 0;
            c1 = - x_mean_1;
            c2 = - x_mean_2;

        }
    }
    else
    {
        a = Dxy / den;
        b = (lambda - Dxx) / den;
        c1 = - a * x_mean_1 - b * y_mean_1;
        c2 = - a * x_mean_2 - b * y_mean_2;

        if(a < 0)
        {
            a = -a;
            b = -b;
            c1 = -c1;
            c2 = -c2;
        }
    }
    return true;
}

/**
 * @brief weightedPLinesFitOneStep 加权最小二乘直线拟合。根据传进来的 a,b,c1,c2 来估计加权值。
 * @param line1 第一条直线的数据点
 * @param line2 第二条直线的数据点
 * @param [inout] a 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] b 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] c1 输出拟合系数，第一条直线的方程为 ax + by + c1 = 0
 * @param [inout] c2 输出拟合系数，第二条直线的方程为 ax + by + c2 = 0
 * @return true 表示拟合成功，false 表示拟合失败
 */
bool weightedPLinesFitOneStep(const QVector<QPointF> &line1, const QVector<QPointF> &line2,
                      double &a, double &b, double &c1, double &c2, double tau)
{
    if(line1.size() < 2 ||  line2.size() < 0)
    {
        a = 0;
        b = 0;
        c1 = 0;
        c2 = 0;
        return false;
    }

    double x_mean_1 = 0, x_mean_2 = 0;
    double y_mean_1 = 0, y_mean_2 = 0;
    weightedMean_xy(line1, x_mean_1, y_mean_1, tau, a, b, c1);
    weightedMean_xy(line2, x_mean_2, y_mean_2, tau, a, b, c2);

    double Dxx = 0, Dxy = 0, Dyy = 0;
    weightedDxxxyyy(line1, x_mean_1, y_mean_1, Dxx, Dxy, Dyy, tau, a, b, c1);
    weightedDxxxyyy(line2, x_mean_2, y_mean_2, Dxx, Dxy, Dyy, tau, a, b, c2);

    double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
    double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );

    if(fabs(den) < 1e-5)
    {
        if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向，无法拟合
        {
            return false;
        }
        else
        {
            a = 1;
            b = 0;
            c1 = - a * x_mean_1 - b * y_mean_1;
            c2 = - a * x_mean_2 - b * y_mean_2;
        }
    }
    else
    {
        a = Dxy / den;
        b = (lambda - Dxx) / den;
        c1 = - a * x_mean_1 - b * y_mean_1;
        c2 = - a * x_mean_2 - b * y_mean_2;
        if(a < 0)
        {
            a = -a;
            b = -b;
            c1 = -c1;
            c2 = -c2;
        }
    }
    return true;

}


/**
 * @brief weightedPLinesFit 加权最小二乘直线拟合。先用非加权方法估算出个直线方程，然后再迭代优化。
 * @param line1 第一条直线的数据点
 * @param line2 第二条直线的数据点
 * @param [inout] a 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] b 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] c1 输出拟合系数，第一条直线的方程为 ax + by + c1 = 0
 * @param [inout] c2 输出拟合系数，第二条直线的方程为 ax + by + c2 = 0
 * @param tau 距离小于 tau 的权重为 1。距离大于 tau 的权重为 tau / d。 d 为点到直线距离。
 * @param N 迭代次数
 * @return true 表示拟合成功，false 表示拟合失败
 */
bool weightedPLinesFit(const QVector<QPointF> &line1, const QVector<QPointF> &line2,
                      double &a, double &b, double &c1, double &c2, double tau, int N)
{
    bool ret;
    ret = plinesFit(line1, line2, a, b, c1, c2);
    if( !ret ) return false;
    for(int i = 0; i < N; i++)
    {
        ret = weightedPLinesFitOneStep(line1, line2, a, b, c1, c2, tau);
        if( !ret ) return false;
    }
    return true;
}

/**
 * @brief lineFit 最小二乘法拟合 ax + by + c = 0 型直线.
 *  参数 a 和 b 满足 a^2 + b^2 = 1。拟合的判据是点到直线的垂直距离平方和最小。不是通常意义下的一元线性回归。
 * @param points 点的坐标
 * @param [out] a 输出拟合系数
 * @param [out] b 输出拟合系数
 * @param [out] c 输出拟合系数
 */
bool lineFit(const QVector<QPointF> &points, double &a, double &b, double &c)
{
    int size = points.size();
    if(size < 2)
    {
        a = 0;
        b = 0;
        c = 0;
        return false;
    }
    double x_mean = 0;
    double y_mean = 0;
    mean_xy(points, x_mean, y_mean);

    double Dxx = 0, Dxy = 0, Dyy = 0;
    Dxxyyxy(points, x_mean, y_mean, Dxx, Dxy, Dyy);

    double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
    double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );

    if(fabs(den) < 1e-5)
    {
        if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向，无法拟合
        {
            return false;
        }
        else
        {
            a = 1;
            b = 0;
            c = - x_mean;
        }
    }
    else
    {
        a = Dxy / den;
        b = (lambda - Dxx) / den;
        c = - a * x_mean - b * y_mean;
    }
    return true;
}

/**
 * @brief weightedLineFitOneStep 加权最小二乘直线拟合。根据传进来的 a,b,c 来估计加权值。
 * @param [inout] a 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] b 输出拟合系数, 同时也作为估计加权系数的依据
 * @param [inout] c 输出拟合系数，直线的方程为 ax + by + c = 0
 * @param tau 距离小于 tau 的权重为 1。距离大于 tau 的权重为 tau / d。 d 为点到直线距离。
 * @return true 表示拟合成功，false 表示拟合失败
 */
bool weightedLineFitOneStep(const QVector<QPointF> &line, double &a, double &b, double &c, double tau)
{
    int size = line.size();
    if(size < 2)
    {
        a = 0;
        b = 0;
        c = 0;
        return false;
    }
    double x_mean = 0;
    double y_mean = 0;
    weightedMean_xy(line, x_mean, y_mean, tau, a, b, c);
    double Dxx = 0, Dxy = 0, Dyy = 0;
    weightedDxxxyyy(line, x_mean, y_mean, Dxx, Dxy, Dyy, tau, a, b, c);

    double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
    double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );

    if(fabs(den) < 1e-5)
    {
        if( fabs(Dxx / Dyy - 1) < 1e-5) //这时没有一个特殊的直线方向，无法拟合
        {
            return false;
        }
        else
        {
            a = 1;
            b = 0;
            c = - a * x_mean - b * y_mean;
        }
    }
    else
    {
        a = Dxy / den;
        b = (lambda - Dxx) / den;
        c = - a * x_mean - b * y_mean;
    }
    return true;
}

/**
 * @brief weightedLineFit 加权最小二乘法拟合 ax + by + c = 0 型直线。
 * 参数 a 和 b 满足 a^2 + b^2 = 1。拟合的判据是点到直线的垂直距离平方和最小。不是通常意义下的一元线性回归。
 * 权重根据点到直线的距离来确定。距离小于 tau 的权重为 1。距离大于 tau 的权重为 tau / d。 d 为点到直线距离。
 * @param line 点的坐标
 * @param [out] a 输出拟合系数
 * @param [out] b 输出拟合系数
 * @param [out] c 输出拟合系数
 * @param tau 距离小于 tau 的权重为 1。距离大于 tau 的权重为 tau / d。 d 为点到直线距离。
 * @param N 迭代次数
 * @return true 表示拟合成功，false 表示拟合失败
 */
bool weightedLineFit(const QVector<QPointF> &line, double &a, double &b, double &c, double tau, int N)
{
    bool ret;
    ret = lineFit(line, a, b, c);
    if( !ret ) return false;
    for(int i = 0; i < N; i++)
    {
        ret = weightedLineFitOneStep(line, a, b, c, tau);
        if( !ret ) return false;
    }
    return true;
}
